
#include <stdlib.h>
#include <string.h>

#include "Control.h"
#include "Traversing.h"
#include "Ultrasonic.h"
#include "adc.h"
#include "beep.h"
#include "common.h"
#include "delay.h"  //包含系统定时器库头文件
#include "encoder.h"
#include "key.h"
#include "led.h"
#include "motor.h"
#include "stm32f10x.h"  //包含stm32库系统头文件
#include "timer.h"      //包含定时器设置头文件
#include "usart.h"      //包含串口通信设置头文件
u32 value;
u32  key_bak;
u8 key_mode;
u8 ck = 0,temp = 0;
extern uint8 redata3[];
Motor motorLeft = {0}, motorRight = {0}, *motorArr[2];
u8 sand[]={'A','S','C','1','2'};
// todo 根据实际的超声波接线，初始化超声波传感器
UltrasonicDevice usDevCenter = {.clockIn = RCC_APB2Periph_GPIOC,
                              .clockOut = RCC_APB2Periph_GPIOC,
                              .portIn = GPIOC,
                              .portOut = GPIOC,
                              .pinIn = GPIO_Pin_6,
                              .pinOut = GPIO_Pin_7},
                 usDevRight ,
															usDevLeft,
								  *usDevArr[US_SENSOR_NUM];
extern int Velocity_KP, Velocity_KI;  // 速度控制PID参数
extern uint8 flag_RecFul;
char redata[500] = {0};  // 定义接收数据变量数组

void InitMotors(void) {
    int i;
    int LowSpeedLimit = 600, HighSpeedLimit = 1000;
    motorArr[MOTOR_LEFT] = &motorLeft;
    motorArr[MOTOR_RIGHT] = &motorRight;
    for (i = 0; i < 2; i++) {
        Motor *pMotor = motorArr[i];
        memset(pMotor, 0, sizeof(Motor));
        pMotor->id = i;
        pMotor->GetEncoder = Read_Encoder;
        pMotor->SetTargetEncoder = SetMotorSpeed;
        pMotor->SetLimit = SetMotorSpeedLimit;
        pMotor->SetMotorPwm = SetMotorPwm;
        pMotor->CalcPIDResult = Incremental_PI;
        pMotor->LimitPwmDuty = LimitPwmDuty;
        pMotor->SetLimit(pMotor, LowSpeedLimit, HighSpeedLimit);
    }
}

void InitUltrasonicDevices() {
    int i;
    usDevArr[US_LEFT] = &usDevLeft;
    usDevArr[US_CENTER] = &usDevCenter;
    usDevArr[US_RIGHT] = &usDevRight;
//    for (i = 0; i < 3; i++) {
        UltrasonicDevice *pUs = usDevArr[0];
        pUs->lastCNT = 0;
        pUs->distance = 0;
        pUs->Init = InitUltrasonic;
        pUs->EmitDectSingal = EmitDectSingal;
        pUs->Init(pUs);
//    }
}

/**********************************************************************************/
/*****************************主函数***********************************************/
int main(void) {
    LED_Init();
		
    Traversing_GPIO_Init();
    KEY_Init();
    Beep_Init();
    Uart_Init(1);
		Uart_Init(3);
    // OLED_Init();                    //=====OLED初始化
    USART_Config(USART1, 115200);
    USART_Config(USART3, 115200);
    Motor_Gpio_init();
    PWM_Init(1000, 5);  // 初始化电机工作频率 72/(5+1)  12KHZ
    InitMotors();
    Encoder_Init_TIM3();
    Encoder_Init_TIM_Exit0();
    Encoder_Init_TIM_Exit1();
    Led_Test();
    Timer_Init();  // 定时器初始化
    Timer_ON();    // 开启定时器
    InitUltrasonicDevices();
		USART_SendData(USART3,sand[0]);
		Beep_Test();
		key_mode = KEY_Scan();
    while (1) {
							
//        printf("%d,%d,%d,%d,%d,%d\n", motorLeft.targetEncoder, motorLeft.speed,
//               motorLeft.encoder,motorRight.targetEncoder, motorRight.speed, motorRight.encoder);
        Auto_Traversing();
    }
}
